

// This file contains sample code to help application developers
// interface with Garmin USB units. This should not be viewed as a
// full implementation of PC to unit communication. It does not include
// error checking and other elements of a full implementation.
// Also, there are notes in the code suggesting other elements that
// might be necessary.

#include <stdio.h>
#include <tchar.h>

#include <windows.h>
#include <initguid.h>
#include <setupapi.h> // You may need to explicitly link with setupapi.lib
#include <winioctl.h>

DEFINE_GUID(GUID_DEVINTERFACE_GRMNUSB, 0x2c9c45c2L, 0x8e7d, 0x4c08, 0xa1, 0x2d, 0x81, 0x6b, 0xba, 0xe7, 0x22, 0xc0);

#define IOCTL_ASYNC_IN        CTL_CODE (FILE_DEVICE_UNKNOWN, 0x850, METHOD_BUFFERED, FILE_ANY_ACCESS)
#define IOCTL_USB_PACKET_SIZE CTL_CODE (FILE_DEVICE_UNKNOWN, 0x851, METHOD_BUFFERED, FILE_ANY_ACCESS)

#define MAX_BUFFER_SIZE 4096
#define ASYNC_DATA_SIZE 64

//-----------------------------------------------------------------------------
HANDLE gHandle;
DWORD gUSBPacketSize;

typedef unsigned __int8 uint8;
typedef unsigned __int16 uint16;
typedef signed __int16	sint16;

//-----------------------------------------------------------------------------
#pragma pack( push, 1)
typedef struct
    {
    unsigned char  mPacketType;
    unsigned char  mReserved1;
    unsigned short mReserved2;
    unsigned short mPacketId;
    unsigned short mReserved3;
    unsigned long  mDataSize;
    BYTE           mData[2];
    } Packet_t;
#pragma pack( pop)

#pragma pack( push, 1)
typedef struct
{
	float alt; /* altitude above WGS 84 ellipsoid (meters) */
	float epe; /* estimated position error, 2 sigma (meters) */
	float eph; /* epe, but horizontal only (meters) */
	float epv; /* epe, but vertical only (meters) */
	int fix; /* type of position fix */
	long float tow; /* time of week (seconds) */
	int posn; /* latitude and longitude (radians) */
	float east; /* velocity east (meters/second) */
	float north; /* velocity north (meters/second) */
	float up; /* velocity up (meters/second) */
	float msl_hght; /* height of WGS84 ellipsoid above MSL(meters)*/
	int leap_scnds; /* difference between GPS and UTC (seconds) */
	long intwn_days; /* week number days */
} D800_Pvt_Data_Type;
#pragma pack( pop)

// The following structs are all found in the
// GPS 18 Technical Specifications Revision D
// http://www8.garmin.com/manuals/425_TechnicalSpecification.pdf

// Satellite Record Data
// This is found on page 25 (Appendix B)
#pragma pack( push, 1)
typedef struct
{
	uint8		svid;	// space vehicle identification
	uint16		snr;	// signal-to-noise ratio
	uint8		elev;	// satellite elevation in degrees
	uint16		azmth;	// satellite azimuth in degrees
	uint8		status;	// status bit-field
} cpo_sat_data;
#pragma pack( pop)

#pragma pack( push, 1)
typedef struct
{
	cpo_sat_data	sv[12];
} cpo_all_sat_data;
#pragma pack( pop)

// Position Record Data
// This is found on page 26 (Appendix B)
#pragma pack( push, 1)
typedef struct
{
	float	alt;		// Ellipsoid altitude (meters)
	float	epe;		// Est pos error (meters)
	float	eph;		// Pos err, horizontal (meters)
	float	epv;		// Pos err, vertical (meters)
	short	fix;		// 0 = no fix; 1=no fix; 2=2D; 3=3D; 4=2D
	double	gps_tow;	// GPS time of week (sec)
	double	lat;		// Latitude (radians)
	double	lon;		// Longitude (radians)
	float	lon_vel;	// Longitude velocity (meters/second)
	float	lat_vel;	// Latitude velocity (meters/second)
	float	msl_hght;	// Mean sea level height (meters)
	short	leap_sec;	// UTC leap seconds
	long	grmn_days;	// Garmin days (days since December 31, 1989)
} cpo_pvt_data;
#pragma pack( pop)

#pragma pack( push, 1)
typedef struct
{
	cpo_all_sat_data	sat;
	cpo_pvt_data		pvt;
} cpo_pvt_sat;
#pragma pack( pop)

// Receiver Measurement Record Data
// This is found on page 27 (Appendix B)
#pragma pack( push, 1)
typedef struct
{
	unsigned long	cycles;		// Number of Accumulated Cycles
	double			pr;			// Pseudo Range
	unsigned short	phase;		// 
	char			slp_dtct;	// 0=no cycle slip detected
	unsigned char	snr_dbhz;	// Signal strength -db Hz
	uint8			svid;		// Satellite Number
	uint8			valid;		// 0=information not valid
} cpo_rcv_sv_data;
#pragma pack( pop)

#pragma pack( push, 1)
typedef struct
{
	double			rcvr_tow;	// Receiver time of week (seconds)
	uint16			rcvr_wn;	// Receiver week number
	cpo_rcv_sv_data	sv[12];		// 
} cpo_rcv_data;
#pragma pack( pop)


// Ephemeris Data
// This is found on page 28 (Appendix C)
// GPS 16/17 Technical Specifications Revision A
// http://www8.garmin.com/manuals/470_GPS16_17TechnicalSpecification.pdf
#pragma pack( push, 1)
typedef struct
{
	sint16		wn;		// week number (weeks}
	float		toc;	// reverence time of clock parameters (s)
	float		toe;	// reference time of ephemeris parameters (s)
	float		af0;	// clock correction coefficient - group delay (s)
	float		af1;	// clock correction coefficient (s/s)
	float		af2;	// clock correction coefficient (s/s/s)
	float		ura;	// user range accuracy (m)
	double		e;		// eccentricity
	double		sqrta;	// square root of semi-major axis (m**1/2)
	double		dn;		// mean motion correction  (r/s)
	double		m0;		// mean anomaly at reference time (r)
	double		w;		// argument of perigee (r)
	double		omg0;	// right ascension (r)
	double		i0;		// inclination angle at reference time (r)
	float		odot;	// rate of right ascension (r/s)
	float		idot;	// rate of inclination angle (r/s)
	float		cus;	// argument of latitude correction, sine (r)
	float		cuc;	// argument of latitude correction, cosine (r)
	float		cis;	// inclination correction, sine (r)
	float		cic;	// inclination correction, cosine (r)
	float		crs;	// radius correction, sine (m)
	float		crc;	// radius correction, cosin (m)
	unsigned char	iod; // issue of data
} cpo_eph_data;
#pragma pack( pop)

#pragma pack( push, 1)
typedef struct
{
	cpo_eph_data	sv[12];
} cpo_all_eph_data;
#pragma pack( pop)

#pragma pack( push, 1)
typedef struct
{
	cpo_eph_data	sv[32];
} eph_data_all;
#pragma pack( pop)

// Some generic packets
Packet_t StartPVTPacket =		{20, 0, 0, 10, 0, 4, {49,0}};
Packet_t StopPVTPacket =		{20, 0, 0, 10, 0, 4, {50,0}};
Packet_t StartSessionPacket =	{ 0, 0, 0, 5, 0 , 0 };
Packet_t ProductDataPacket =	{ 20, 0, 0, 254, 0 , 0 , {0, 0}}; 
Packet_t PositionPacket =       { 20, 0, 0, 10 , 0 , 2 , {2, 0}}; 
Packet_t TimePacket =			{ 20, 0, 0,  14, 0 , 2 , {2, 0}}; 

  //-----------------------------------------------------------------------------
void SendPacket(Packet_t aPacket) {
	DWORD theBytesToWrite = sizeof( aPacket ) - 1 + aPacket.mDataSize;
	DWORD theBytesReturned = 0;

	WriteFile( gHandle,
			   &aPacket,
			   theBytesToWrite,
			   &theBytesReturned,
			   NULL );

	// If the packet size was an exact multiple of the USB packet
	// size, we must make a final write call with no data
	if( theBytesToWrite % gUSBPacketSize == 0 )
		{
		WriteFile( gHandle,
				   0,
				   0,
				   &theBytesReturned,
				   NULL );
		}
}

//-----------------------------------------------------------------------------
// Gets a single packet. Since packets may come simultaneously through
// asynchrous reads and normal (ReadFile) reads, a full implementation
// may require a packet queue and multiple threads.
	Packet_t* GetPacket() {
	Packet_t* thePacket = 0;
	DWORD theBufferSize = 0;
	BYTE* theBuffer = 0;

	for( ; ; )
		{
		// Read async data until the driver returns less than the
		// max async data size, which signifies the end of a packet
		BYTE theTempBuffer[ASYNC_DATA_SIZE];
		BYTE* theNewBuffer = 0;
		DWORD theBytesReturned = 0;

		DeviceIoControl( gHandle,
						 IOCTL_ASYNC_IN,
						 0,
						 0,
						 theTempBuffer,
						 sizeof( theTempBuffer ),
						 &theBytesReturned,
						 NULL );

		theBufferSize += ASYNC_DATA_SIZE;
		theNewBuffer = (BYTE*) malloc( theBufferSize );
		memcpy( theNewBuffer, theBuffer, theBufferSize - ASYNC_DATA_SIZE );
		memcpy( theNewBuffer + theBufferSize - ASYNC_DATA_SIZE,
				theTempBuffer,
				ASYNC_DATA_SIZE );

		free( theBuffer );

		theBuffer = theNewBuffer;

		if( theBytesReturned != ASYNC_DATA_SIZE )
			{
			thePacket = (Packet_t*) theBuffer;
			break;
			}
		}

	// If this was a small "signal" packet, read a real
	// packet using ReadFile
	if( thePacket->mPacketType == 0 &&
		thePacket->mPacketId == 2 )
		{
		BYTE* theNewBuffer = (BYTE*) malloc( MAX_BUFFER_SIZE );
		DWORD theBytesReturned = 0;

		free( thePacket );

		// A full implementation would keep reading (and queueing)
		// packets until the driver returns a 0 size buffer.
		ReadFile( gHandle,
				  theNewBuffer,
				  MAX_BUFFER_SIZE,
				  &theBytesReturned,
				  NULL );

		return (Packet_t*) theNewBuffer;
		}
	else
		{
		return thePacket;
		}
}

//-----------------------------------------------------------------------------
void
Initialize() {
	// Make all the necessary Windows calls to get a handle
	// to our USB device
	DWORD theBytesReturned = 0;

	PSP_INTERFACE_DEVICE_DETAIL_DATA theDevDetailData = 0;
	SP_DEVINFO_DATA theDevInfoData = { sizeof( SP_DEVINFO_DATA ) };

	Packet_t* thePacket = 0;

	HDEVINFO theDevInfo = SetupDiGetClassDevs(
		(GUID*) &GUID_DEVINTERFACE_GRMNUSB,
		NULL,
		NULL,
		DIGCF_PRESENT | DIGCF_INTERFACEDEVICE );

	SP_DEVICE_INTERFACE_DATA theInterfaceData;
	theInterfaceData.cbSize = sizeof( theInterfaceData );

	if( !SetupDiEnumDeviceInterfaces( theDevInfo,
									  NULL,
									  (GUID*) &GUID_DEVINTERFACE_GRMNUSB,
									  0,
									  &theInterfaceData ) &&
		GetLastError() == ERROR_NO_MORE_ITEMS )
		{
		gHandle = 0;
		return;
		}


	SetupDiGetDeviceInterfaceDetail(
		theDevInfo,
		&theInterfaceData,
		NULL,
		0,
		&theBytesReturned,
		NULL );

	theDevDetailData =
		(PSP_INTERFACE_DEVICE_DETAIL_DATA) malloc( theBytesReturned );
	theDevDetailData->cbSize = sizeof( SP_INTERFACE_DEVICE_DETAIL_DATA );

	SetupDiGetDeviceInterfaceDetail( theDevInfo,
									 &theInterfaceData,
									 theDevDetailData,
									 theBytesReturned,
									 NULL,
									 &theDevInfoData );

	gHandle = CreateFile(
		theDevDetailData->DevicePath,
		GENERIC_READ | GENERIC_WRITE,
		0,
		NULL,
		OPEN_EXISTING,
		FILE_ATTRIBUTE_NORMAL,
		NULL );

	free( theDevDetailData );

	// Get the USB packet size, which we need for sending packets
	DeviceIoControl( gHandle,
					 IOCTL_USB_PACKET_SIZE,
					 0,
					 0,
					 &gUSBPacketSize,
					 sizeof( gUSBPacketSize ),
					 &theBytesReturned,
					 NULL );

	// Tell the device that we are starting a session.
	SendPacket( StartSessionPacket );

	// Wait until the device is ready to the start the session
	for( ; ; )
		{
		thePacket = GetPacket();

		if( thePacket->mPacketType == 0 &&
			thePacket->mPacketId == 6 )
			{
			break;
			}

		free( thePacket );
		}

	free( thePacket );
}


cpo_pvt_sat GetPVTData (){
	Packet_t* thePacket = NULL;
	cpo_pvt_data *pvt=NULL;
	cpo_pvt_data pvt2;
	cpo_all_sat_data *sat=NULL;
	cpo_all_sat_data sat2;
	cpo_pvt_sat  data;

	int fin = 0;
	
	SendPacket(StartPVTPacket);
	//Sleep(50);
	//SendPacket(StartPVTPacket);

	while (fin==0){
		thePacket = GetPacket();
		if( thePacket->mPacketType == 20){	// Application
			switch (thePacket->mPacketId){
			case 51:
			{
				pvt=(cpo_pvt_data*)thePacket->mData;
				pvt2=*pvt;
				pvt=NULL;

				thePacket = GetPacket();
				if( thePacket->mPacketType == 20){	// Application
					switch(thePacket->mPacketId){
					case 114:
						{
							sat = (cpo_all_sat_data*)thePacket->mData;
							sat2 = *sat;
							sat = NULL;

							fin = 1;
						}
						break;
					default:
					{
						printf("Packet Type is %i GetPVTData\n",(int)thePacket->mPacketId);
						//SendPacket(StopTransPacket);
						//Sleep(50);
						//SendPacket(StopRawPacket);
						//Sleep(50);
						SendPacket(StartPVTPacket);
					}
					break;
					}
					thePacket = NULL;
				}
			}
			break;

			case 114:
			{
				sat = (cpo_all_sat_data*)thePacket->mData;
				sat2 = *sat;
				sat = NULL;

				thePacket = GetPacket();
				if( thePacket->mPacketType == 20){	// Application
					switch (thePacket->mPacketId){
					case 51:
					{
						pvt = (cpo_pvt_data*)thePacket->mData;
						pvt2=*pvt;
						pvt=NULL;

						fin = 1;
					}
					break;

					default:
					{
						printf("Packet Type is %i GetPVTData\n",(int)thePacket->mPacketId);
						//SendPacket(StopTransPacket);
						//Sleep(50);
						//SendPacket(StopRawPacket);
						//Sleep(50);
						SendPacket(StartPVTPacket);
					}
					break;
					}
					thePacket = NULL;
				}
			}
			break;

			default:
			{
				printf("Packet Type is %i GETPvtData\n",(int)thePacket->mPacketId);
				//SendPacket(StopTransPacket);
				//Sleep(50);
				//SendPacket(StopRawPacket);
				//Sleep(50);
				SendPacket(StartPVTPacket);
			}
			break;
			}
			thePacket=NULL;
		}
	}
	//SendPacket(StopPVTPacket);
	//printf("End of PVT Packet Routine\n");

	//sat2 = *sat;
	//sat = NULL;
	data.pvt = pvt2;
	data.sat = sat2;
	return data;
}


void retrieveData() {
	Packet_t* thePacket = 0; 

	thePacket = GetPacket(); 

	printf("Packet Type/ID: %d/%d\r\n", (int)thePacket->mPacketType, (int)thePacket->mPacketId); 
	if(thePacket->mPacketType == 20) { // Application layer
		switch (thePacket->mPacketId) { 
			case 255: { 
				printf( "Product ID: %d, Software Version %d Version:", *(short *)&thePacket->mData[0], *(short *)&thePacket->mData[2]); 
				printf( "%s\r\n", (char*) &thePacket->mData[4] ); 
			} 
			break; 
			case 253:  
			break; 
			case 14:


			case 17: { 
				double p1 = *(double*)&thePacket->mData[0]; 
				double p2 = *(double*)&thePacket->mData[8]; 
						
				p1 *= 180.0; 
				p1 /= 3.14159265358979; 

				p2 *= 180.0; 
				p2 /= 3.14159265358979; 

				printf(" Position %lf, %lf\r\n", (double)p1,(double)p2); 
                
				_sleep (1025); 
			}
			break; 
		}
	}
	free(thePacket); 
}

int _tmain(int argc, _TCHAR* argv[]) 
{ 
   Packet_t* thePacket = 0; 
   cpo_pvt_sat my_pvt;

   Initialize(); 

   if( gHandle == 0 ) 
    { 
      printf( "%s", "No device" ); 
      return 0; 
    } 

/* Tell the device to send product data */ 
   SendPacket( ProductDataPacket ); 

	//
	while(1) { 
			retrieveData();
			my_pvt = GetPVTData();
			printf("LAT=%lf LON=%lf TOW=%lf", my_pvt.pvt.lat*180.0/3.14159265358979, my_pvt.pvt.lon*180.0/3.14159265358979, my_pvt.pvt.gps_tow);
			SendPacket(PositionPacket);
			retrieveData();
		}
	free(thePacket);
	return 0;
} 
